#include "rtbus.h"
#include <pthread.h>
#include <thread>
#include <chrono>
#include "config.h"
#include "sim/simbus.h"
#include "can/jkcan.h"
#ifdef ECAT_ENABLE
#include "ethercat/ecat.h"
#endif
#include "hwdrv/hwdrv.h"
#include "log/zlog.hh"
#include "jkutil/bitdataop.h"
#include "jkutil/configparser.h"
#include "hwdrv/hwdrv.h"
using namespace rtbus;

static pthread_cond_t rtbus_task_cond = PTHREAD_COND_INITIALIZER;
static pthread_mutex_t rtbus_task_mutex = PTHREAD_MUTEX_INITIALIZER;
static int rtbus_ready = 0;

RtCommuItf::~RtCommuItf() {}
int RtCommuItf::config_slave(SlaveInfo& info)
{
    slaves_info_.push_back(info);
    return 0;
}
int RtCommuItf::config_txpdo(uint32_t obj, size_t size)
{
    //
    if (txpdo_.count(obj))
        return -1;
    txpdo_.emplace(obj, PdoObjConfig{obj, size, {0}, true, 0});
    return 0;
}
int RtCommuItf::config_rxpdo(uint32_t obj, size_t size)
{
    //
    if (rxpdo_.count(obj))
        return -1;
    rxpdo_.emplace(obj, PdoObjConfig{obj, size, {0}, false, 0});
    return 0;
}

static BusMgr rtbus_mgr__;
BusMgr::BusMgr() { interval_us_ = 8000; }
BusMgr::~BusMgr() {}
BusMgr* BusMgr::get() { return &rtbus_mgr__; }
std::shared_ptr<RtCommuItf> BusMgr::create_bus(std::string type)
{
    if (type == "sim")
    {
        return std::make_shared<SimBus>();
    }
    else if (type == "ccan")
    {
        return std::make_shared<JkCan>(false);
    }
    else if (type == "scan")
    {
        return std::make_shared<JkCan>(true);
    }
    else if (type == "ecat")
    {
#ifdef ECAT_ENABLE
        return std::make_shared<ECat>();
#else
        return nullptr;
#endif
    }
    else
    {
        return nullptr;
    }
}
int BusMgr::append_bus(std::shared_ptr<RtCommuItf> bus)
{
    std::lock_guard<std::mutex> lock(mtx_);
    bus->set_bus_id(all_bus_.size());
    all_bus_.push_back(bus);
    return all_bus_.size();
}
int BusMgr::init_all_bus()
{
    if (all_bus_.size())
        return 0;
    all_bus_.clear();
    Util::ConfigParser parser(ZUC_DEFAULT_CONFIG_DIR "/rtdev.xml");
    for (int i = 0;; i++)
    {
        auto rtbuspath = Util::ConfigParser::str_append_index("rtbus", i);
        std::string adapter = "sim";
        if (parser.get(rtbuspath + ".adapter", adapter, true))
            break;
        int channel = -1;
        if (parser.get(rtbuspath + ".channel", channel, true) || channel < 0)
            break;
        int dev_id = 0;
        parser.get(rtbuspath + ".dev", dev_id, true);

        auto bus = create_bus(adapter);
        if (!bus)
        {
            continue;
        }
        else
        {
            // 尝试打开所有总线
            if (dev_id)
            {
                bus->set_name(adapter + std::to_string(channel) + "_" + std::to_string(dev_id));
            }
            else
            {
                bus->set_name(adapter + std::to_string(channel));
            }
            int ret = -1;
            for (int i = 0; i < 3; i++)
            {
                RtCommuItf::BusCfg buscfg;
                buscfg.channel_id = channel;
                buscfg.dev_id = dev_id;
                ret = bus->open(buscfg);
                if (!ret)
                    break;
            }
            if (ret)
            {
                zlog()->warn("[RtBus] open bus {} faild {}", bus->get_name(), ret);
            }
            else
            {
                zlog()->info("[RtBus] open bus {} success", bus->get_name());
            }
            append_bus(bus);
        }
    }

    return 0;
}

// static int jkcan_scb_data_map(std::shared_ptr<RtCommuItf> bus)
// {

//     // 索引长度3
//     bus->register_sdo_handle(scb->slave_id(), 0x612, 0x612, 0x592, 3);
//     return 0;
// }

void BusMgr::close()
{
    std::lock_guard<std::mutex> lock(mtx_);
    for (auto& bus : all_bus_) { bus->close(); }
    all_bus_.clear();
}

void BusMgr::update(int64_t* timeoff)
{
    std::lock_guard<std::mutex> lock(mtx_);
    for (auto& bus : all_bus_)
    {
        if (!bus->is_actived())
        {
            continue;
        }
        //对于ethercat类型的双本体总线，需要将两个总线的时间同步。所以将robbus0作为时间参考。
        //注意：scbbus没有同步要求，所以即使同步了也不会产生影响
        if (bus->get_name() == "robbus0")
        {
            bus->recv(timeoff);
        }
        else
        {
            bus->recv(nullptr);
            auto reftime = this->bus("robbus0");
            if (reftime.get() && bus->is_actived())
            {
                bus->sync_time(reftime.get());
            }
        }
        bus->send();
    }
}

void BusMgr::hook_function(std::function<void(void)> f)
{
    if (is_dev_enabled_)
    {
        f();
    }
}

void rtbus::schedule_motion()
{
    pthread_mutex_lock(&rtbus_task_mutex);
    rtbus_ready = 1;
    pthread_cond_broadcast(&rtbus_task_cond);  //触发traj_task运行
    pthread_mutex_unlock(&rtbus_task_mutex);
    // zlog()->debug("[RtBus] Send motion task signal");
}
int rtbus::check_motion_ack() { return -1; }
void rtbus::schedule_wait()
{
    pthread_mutex_lock(&rtbus_task_mutex);
    while (!rtbus_ready) { pthread_cond_wait(&rtbus_task_cond, &rtbus_task_mutex); }  // rtbus_ready防止被虚假唤醒
    rtbus_ready = 0;
    pthread_mutex_unlock(&rtbus_task_mutex);
    // zlog()->debug("[RtBus] Get motion task signal");
}
void rtbus::schedule_send_ack()
{
    //TODO:准备指令数据
    // zlog()->debug("[RtBus] Resp motion task signal");
}

int RtCommuItf::set_txpdo_data(uint32_t obj, uint8_t* data)
{
    if (txpdo_.count(obj) == 0)
        return -1;
    if (txpdo_[obj].size == 0)
        return -1;
    txpdo_[obj].alive_cnt++;
    memcpy(txpdo_[obj].data, data, txpdo_[obj].size);
    return 0;
}
int RtCommuItf::get_rxpdo_data(uint32_t obj, uint8_t* data)
{
    if (rxpdo_.count(obj) == 0)
        return -1;
    if (rxpdo_[obj].size == 0)
        return -1;
    memcpy(data, rxpdo_[obj].data, rxpdo_[obj].size);
    return rxpdo_[obj].alive_cnt;
}
// int RtCommuItf::get_sdo_blocking(uint8_t slave_id, uint32_t objid, void* data, int check_len, int start_index, int data_len, int retry)
// {
//     const uint32_t timeout = 64;
//     bool data_check = false;
//     auto err = get_sdo(
//         slave_id,
//         objid,
//         [data, check_len, start_index, data_len, &data_check](uint8_t* d, int size) {
//             if (size != check_len)
//                 return -1;
//             uint8_t big_end[8] = {};
//             for (int i = 0; i < data_len; i++) { big_end[data_len - i - 1] = d[start_index + i]; }
//             memcpy(data, big_end, data_len);
//             data_check = true;
//             return 0;
//         },
//         timeout,
//         retry);
//     if (err)
//         return err;

//     for (int i = 0; i < retry + 3;)
//     {
//         if (data_check == true)
//         {
//             return 0;
//         }
//         std::this_thread::sleep_for(std::chrono::milliseconds(timeout));
//         if (retry >= 0)
//         {
//             i++;
//         }
//     }
//     return -10;
// }

int RtCommuItf::set_sdo_blocking(uint8_t slave_id, uint32_t objid, void* data, int s, void* respdata, int retry)
{
    const uint32_t timeout = 64;
    bool data_check = false;
    auto err = set_sdo(
        slave_id,
        objid,
        (uint8_t*)data,
        s,
        [&](uint8_t* d, int size) {
            if (size != 8)
                return -1;
            if (respdata)
            {
                memcpy(respdata, d + 4, s);
            }
            data_check = true;
            return 0;
        },
        timeout,
        retry);
    if (err)
    {
        return err;
    }

    for (int i = 0; i < retry + 3;)
    {
        if (data_check == true)
        {
            return 0;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(timeout));
        if (retry >= 0)
        {
            i++;
        }
    }
    return -10;
}
void RtCommuItf::sync_time(RtCommuItf* slave) { ; }
RtCommuItf::BusCfg BusMgr::parser_interface(std::string type, std::string interface)
{
    RtCommuItf::BusCfg ret;
    ret.channel_id = -1;
    ret.dev_id = -1;
    ret.adapter_name = "";
    if (type == "ecat")
    {
        int lanid = std::stoi(interface);
        auto lan = hwdrv::HwMgr::get().get_lan(lanid);
        ret.adapter_name = lan->get_adapter();
    }
    else if (type == "jkcan")
    {
        ret.dev_id = 0;
        ret.channel_id = 0;
        ret.adapter_name = "can0";
    }
    return ret;
}
bool RtCommuItf::is_can() { return true; }
